#include "camodocal/gpl/gpl.h"

#include <set>
#ifdef _WIN32
#include <winsock.h>
#else
#include <time.h>
#endif

// source: https://stackoverflow.com/questions/5167269/clock-gettime-alternative-in-mac-os-x
#ifdef __APPLE__
#include <mach/mach_time.h>
#define ORWL_NANO (+1.0E-9)
#define ORWL_GIGA UINT64_C(1000000000)

static double orwl_timebase = 0.0;
static uint64_t orwl_timestart = 0;

struct timespec orwl_gettime(void)
{
	// be more careful in a multithreaded environement
	if (!orwl_timestart)
	{
		mach_timebase_info_data_t tb = {0};
		mach_timebase_info(&tb);
		orwl_timebase = tb.numer;
		orwl_timebase /= tb.denom;
		orwl_timestart = mach_absolute_time();
	}
	struct timespec t;
	double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase;
	t.tv_sec = diff * ORWL_NANO;
	t.tv_nsec = diff - (t.tv_sec * ORWL_GIGA);
	return t;
}
#endif // __APPLE__

const double WGS84_A = 6378137.0;
const double WGS84_ECCSQ = 0.00669437999013;

// Windows lacks fminf
#ifndef fminf
#define fminf(x, y) (((x) < (y)) ? (x) : (y))
#endif

namespace camodocal
{

	double hypot3(double x, double y, double z)
	{
		return sqrt(square(x) + square(y) + square(z));
	}

	float hypot3f(float x, float y, float z)
	{
		return sqrtf(square(x) + square(y) + square(z));
	}

	double d2r(double deg)
	{
		return deg / 180.0 * M_PI;
	}

	float d2r(float deg)
	{
		return deg / 180.0f * M_PI;
	}

	double r2d(double rad)
	{
		return rad / M_PI * 180.0;
	}

	float r2d(float rad)
	{
		return rad / M_PI * 180.0f;
	}

	double sinc(double theta)
	{
		return sin(theta) / theta;
	}

#ifdef _WIN32
#include <sys/timeb.h>
#include <sys/types.h>
#include <winsock.h>
	LARGE_INTEGER
	getFILETIMEoffset()
	{
		SYSTEMTIME s;
		FILETIME f;
		LARGE_INTEGER t;

		s.wYear = 1970;
		s.wMonth = 1;
		s.wDay = 1;
		s.wHour = 0;
		s.wMinute = 0;
		s.wSecond = 0;
		s.wMilliseconds = 0;
		SystemTimeToFileTime(&s, &f);
		t.QuadPart = f.dwHighDateTime;
		t.QuadPart <<= 32;
		t.QuadPart |= f.dwLowDateTime;
		return (t);
	}

	int
	clock_gettime(int X, struct timespec *tp)
	{
		LARGE_INTEGER t;
		FILETIME f;
		double microseconds;
		static LARGE_INTEGER offset;
		static double frequencyToMicroseconds;
		static int initialized = 0;
		static BOOL usePerformanceCounter = 0;

		if (!initialized)
		{
			LARGE_INTEGER performanceFrequency;
			initialized = 1;
			usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency);
			if (usePerformanceCounter)
			{
				QueryPerformanceCounter(&offset);
				frequencyToMicroseconds = (double)performanceFrequency.QuadPart / 1000000.;
			}
			else
			{
				offset = getFILETIMEoffset();
				frequencyToMicroseconds = 10.;
			}
		}
		if (usePerformanceCounter)
			QueryPerformanceCounter(&t);
		else
		{
			GetSystemTimeAsFileTime(&f);
			t.QuadPart = f.dwHighDateTime;
			t.QuadPart <<= 32;
			t.QuadPart |= f.dwLowDateTime;
		}

		t.QuadPart -= offset.QuadPart;
		microseconds = (double)t.QuadPart / frequencyToMicroseconds;
		t.QuadPart = microseconds;
		tp->tv_sec = t.QuadPart / 1000000;
		tp->tv_nsec = (t.QuadPart % 1000000) * 1000;
		return (0);
	}
#endif

	unsigned long long timeInMicroseconds(void)
	{
		struct timespec tp;
#ifdef __APPLE__
		tp = orwl_gettime();
#else
		clock_gettime(CLOCK_REALTIME, &tp);
#endif

		return tp.tv_sec * 1000000 + tp.tv_nsec / 1000;
	}

	double timeInSeconds(void)
	{
		struct timespec tp;
#ifdef __APPLE__
		tp = orwl_gettime();
#else
		clock_gettime(CLOCK_REALTIME, &tp);
#endif

		return static_cast<double>(tp.tv_sec) +
			   static_cast<double>(tp.tv_nsec) / 1000000000.0;
	}

	float colormapAutumn[128][3] =
		{
			{1.0f, 0.f, 0.f},
			{1.0f, 0.007874f, 0.f},
			{1.0f, 0.015748f, 0.f},
			{1.0f, 0.023622f, 0.f},
			{1.0f, 0.031496f, 0.f},
			{1.0f, 0.03937f, 0.f},
			{1.0f, 0.047244f, 0.f},
			{1.0f, 0.055118f, 0.f},
			{1.0f, 0.062992f, 0.f},
			{1.0f, 0.070866f, 0.f},
			{1.0f, 0.07874f, 0.f},
			{1.0f, 0.086614f, 0.f},
			{1.0f, 0.094488f, 0.f},
			{1.0f, 0.10236f, 0.f},
			{1.0f, 0.11024f, 0.f},
			{1.0f, 0.11811f, 0.f},
			{1.0f, 0.12598f, 0.f},
			{1.0f, 0.13386f, 0.f},
			{1.0f, 0.14173f, 0.f},
			{1.0f, 0.14961f, 0.f},
			{1.0f, 0.15748f, 0.f},
			{1.0f, 0.16535f, 0.f},
			{1.0f, 0.17323f, 0.f},
			{1.0f, 0.1811f, 0.f},
			{1.0f, 0.18898f, 0.f},
			{1.0f, 0.19685f, 0.f},
			{1.0f, 0.20472f, 0.f},
			{1.0f, 0.2126f, 0.f},
			{1.0f, 0.22047f, 0.f},
			{1.0f, 0.22835f, 0.f},
			{1.0f, 0.23622f, 0.f},
			{1.0f, 0.24409f, 0.f},
			{1.0f, 0.25197f, 0.f},
			{1.0f, 0.25984f, 0.f},
			{1.0f, 0.26772f, 0.f},
			{1.0f, 0.27559f, 0.f},
			{1.0f, 0.28346f, 0.f},
			{1.0f, 0.29134f, 0.f},
			{1.0f, 0.29921f, 0.f},
			{1.0f, 0.30709f, 0.f},
			{1.0f, 0.31496f, 0.f},
			{1.0f, 0.32283f, 0.f},
			{1.0f, 0.33071f, 0.f},
			{1.0f, 0.33858f, 0.f},
			{1.0f, 0.34646f, 0.f},
			{1.0f, 0.35433f, 0.f},
			{1.0f, 0.3622f, 0.f},
			{1.0f, 0.37008f, 0.f},
			{1.0f, 0.37795f, 0.f},
			{1.0f, 0.38583f, 0.f},
			{1.0f, 0.3937f, 0.f},
			{1.0f, 0.40157f, 0.f},
			{1.0f, 0.40945f, 0.f},
			{1.0f, 0.41732f, 0.f},
			{1.0f, 0.4252f, 0.f},
			{1.0f, 0.43307f, 0.f},
			{1.0f, 0.44094f, 0.f},
			{1.0f, 0.44882f, 0.f},
			{1.0f, 0.45669f, 0.f},
			{1.0f, 0.46457f, 0.f},
			{1.0f, 0.47244f, 0.f},
			{1.0f, 0.48031f, 0.f},
			{1.0f, 0.48819f, 0.f},
			{1.0f, 0.49606f, 0.f},
			{1.0f, 0.50394f, 0.f},
			{1.0f, 0.51181f, 0.f},
			{1.0f, 0.51969f, 0.f},
			{1.0f, 0.52756f, 0.f},
			{1.0f, 0.53543f, 0.f},
			{1.0f, 0.54331f, 0.f},
			{1.0f, 0.55118f, 0.f},
			{1.0f, 0.55906f, 0.f},
			{1.0f, 0.56693f, 0.f},
			{1.0f, 0.5748f, 0.f},
			{1.0f, 0.58268f, 0.f},
			{1.0f, 0.59055f, 0.f},
			{1.0f, 0.59843f, 0.f},
			{1.0f, 0.6063f, 0.f},
			{1.0f, 0.61417f, 0.f},
			{1.0f, 0.62205f, 0.f},
			{1.0f, 0.62992f, 0.f},
			{1.0f, 0.6378f, 0.f},
			{1.0f, 0.64567f, 0.f},
			{1.0f, 0.65354f, 0.f},
			{1.0f, 0.66142f, 0.f},
			{1.0f, 0.66929f, 0.f},
			{1.0f, 0.67717f, 0.f},
			{1.0f, 0.68504f, 0.f},
			{1.0f, 0.69291f, 0.f},
			{1.0f, 0.70079f, 0.f},
			{1.0f, 0.70866f, 0.f},
			{1.0f, 0.71654f, 0.f},
			{1.0f, 0.72441f, 0.f},
			{1.0f, 0.73228f, 0.f},
			{1.0f, 0.74016f, 0.f},
			{1.0f, 0.74803f, 0.f},
			{1.0f, 0.75591f, 0.f},
			{1.0f, 0.76378f, 0.f},
			{1.0f, 0.77165f, 0.f},
			{1.0f, 0.77953f, 0.f},
			{1.0f, 0.7874f, 0.f},
			{1.0f, 0.79528f, 0.f},
			{1.0f, 0.80315f, 0.f},
			{1.0f, 0.81102f, 0.f},
			{1.0f, 0.8189f, 0.f},
			{1.0f, 0.82677f, 0.f},
			{1.0f, 0.83465f, 0.f},
			{1.0f, 0.84252f, 0.f},
			{1.0f, 0.85039f, 0.f},
			{1.0f, 0.85827f, 0.f},
			{1.0f, 0.86614f, 0.f},
			{1.0f, 0.87402f, 0.f},
			{1.0f, 0.88189f, 0.f},
			{1.0f, 0.88976f, 0.f},
			{1.0f, 0.89764f, 0.f},
			{1.0f, 0.90551f, 0.f},
			{1.0f, 0.91339f, 0.f},
			{1.0f, 0.92126f, 0.f},
			{1.0f, 0.92913f, 0.f},
			{1.0f, 0.93701f, 0.f},
			{1.0f, 0.94488f, 0.f},
			{1.0f, 0.95276f, 0.f},
			{1.0f, 0.96063f, 0.f},
			{1.0f, 0.9685f, 0.f},
			{1.0f, 0.97638f, 0.f},
			{1.0f, 0.98425f, 0.f},
			{1.0f, 0.99213f, 0.f},
			{1.0f, 1.0f, 0.0f}};

	float colormapJet[128][3] =
		{
			{0.0f, 0.0f, 0.53125f},
			{0.0f, 0.0f, 0.5625f},
			{0.0f, 0.0f, 0.59375f},
			{0.0f, 0.0f, 0.625f},
			{0.0f, 0.0f, 0.65625f},
			{0.0f, 0.0f, 0.6875f},
			{0.0f, 0.0f, 0.71875f},
			{0.0f, 0.0f, 0.75f},
			{0.0f, 0.0f, 0.78125f},
			{0.0f, 0.0f, 0.8125f},
			{0.0f, 0.0f, 0.84375f},
			{0.0f, 0.0f, 0.875f},
			{0.0f, 0.0f, 0.90625f},
			{0.0f, 0.0f, 0.9375f},
			{0.0f, 0.0f, 0.96875f},
			{0.0f, 0.0f, 1.0f},
			{0.0f, 0.03125f, 1.0f},
			{0.0f, 0.0625f, 1.0f},
			{0.0f, 0.09375f, 1.0f},
			{0.0f, 0.125f, 1.0f},
			{0.0f, 0.15625f, 1.0f},
			{0.0f, 0.1875f, 1.0f},
			{0.0f, 0.21875f, 1.0f},
			{0.0f, 0.25f, 1.0f},
			{0.0f, 0.28125f, 1.0f},
			{0.0f, 0.3125f, 1.0f},
			{0.0f, 0.34375f, 1.0f},
			{0.0f, 0.375f, 1.0f},
			{0.0f, 0.40625f, 1.0f},
			{0.0f, 0.4375f, 1.0f},
			{0.0f, 0.46875f, 1.0f},
			{0.0f, 0.5f, 1.0f},
			{0.0f, 0.53125f, 1.0f},
			{0.0f, 0.5625f, 1.0f},
			{0.0f, 0.59375f, 1.0f},
			{0.0f, 0.625f, 1.0f},
			{0.0f, 0.65625f, 1.0f},
			{0.0f, 0.6875f, 1.0f},
			{0.0f, 0.71875f, 1.0f},
			{0.0f, 0.75f, 1.0f},
			{0.0f, 0.78125f, 1.0f},
			{0.0f, 0.8125f, 1.0f},
			{0.0f, 0.84375f, 1.0f},
			{0.0f, 0.875f, 1.0f},
			{0.0f, 0.90625f, 1.0f},
			{0.0f, 0.9375f, 1.0f},
			{0.0f, 0.96875f, 1.0f},
			{0.0f, 1.0f, 1.0f},
			{0.03125f, 1.0f, 0.96875f},
			{0.0625f, 1.0f, 0.9375f},
			{0.09375f, 1.0f, 0.90625f},
			{0.125f, 1.0f, 0.875f},
			{0.15625f, 1.0f, 0.84375f},
			{0.1875f, 1.0f, 0.8125f},
			{0.21875f, 1.0f, 0.78125f},
			{0.25f, 1.0f, 0.75f},
			{0.28125f, 1.0f, 0.71875f},
			{0.3125f, 1.0f, 0.6875f},
			{0.34375f, 1.0f, 0.65625f},
			{0.375f, 1.0f, 0.625f},
			{0.40625f, 1.0f, 0.59375f},
			{0.4375f, 1.0f, 0.5625f},
			{0.46875f, 1.0f, 0.53125f},
			{0.5f, 1.0f, 0.5f},
			{0.53125f, 1.0f, 0.46875f},
			{0.5625f, 1.0f, 0.4375f},
			{0.59375f, 1.0f, 0.40625f},
			{0.625f, 1.0f, 0.375f},
			{0.65625f, 1.0f, 0.34375f},
			{0.6875f, 1.0f, 0.3125f},
			{0.71875f, 1.0f, 0.28125f},
			{0.75f, 1.0f, 0.25f},
			{0.78125f, 1.0f, 0.21875f},
			{0.8125f, 1.0f, 0.1875f},
			{0.84375f, 1.0f, 0.15625f},
			{0.875f, 1.0f, 0.125f},
			{0.90625f, 1.0f, 0.09375f},
			{0.9375f, 1.0f, 0.0625f},
			{0.96875f, 1.0f, 0.03125f},
			{1.0f, 1.0f, 0.0f},
			{1.0f, 0.96875f, 0.0f},
			{1.0f, 0.9375f, 0.0f},
			{1.0f, 0.90625f, 0.0f},
			{1.0f, 0.875f, 0.0f},
			{1.0f, 0.84375f, 0.0f},
			{1.0f, 0.8125f, 0.0f},
			{1.0f, 0.78125f, 0.0f},
			{1.0f, 0.75f, 0.0f},
			{1.0f, 0.71875f, 0.0f},
			{1.0f, 0.6875f, 0.0f},
			{1.0f, 0.65625f, 0.0f},
			{1.0f, 0.625f, 0.0f},
			{1.0f, 0.59375f, 0.0f},
			{1.0f, 0.5625f, 0.0f},
			{1.0f, 0.53125f, 0.0f},
			{1.0f, 0.5f, 0.0f},
			{1.0f, 0.46875f, 0.0f},
			{1.0f, 0.4375f, 0.0f},
			{1.0f, 0.40625f, 0.0f},
			{1.0f, 0.375f, 0.0f},
			{1.0f, 0.34375f, 0.0f},
			{1.0f, 0.3125f, 0.0f},
			{1.0f, 0.28125f, 0.0f},
			{1.0f, 0.25f, 0.0f},
			{1.0f, 0.21875f, 0.0f},
			{1.0f, 0.1875f, 0.0f},
			{1.0f, 0.15625f, 0.0f},
			{1.0f, 0.125f, 0.0f},
			{1.0f, 0.09375f, 0.0f},
			{1.0f, 0.0625f, 0.0f},
			{1.0f, 0.03125f, 0.0f},
			{1.0f, 0.0f, 0.0f},
			{0.96875f, 0.0f, 0.0f},
			{0.9375f, 0.0f, 0.0f},
			{0.90625f, 0.0f, 0.0f},
			{0.875f, 0.0f, 0.0f},
			{0.84375f, 0.0f, 0.0f},
			{0.8125f, 0.0f, 0.0f},
			{0.78125f, 0.0f, 0.0f},
			{0.75f, 0.0f, 0.0f},
			{0.71875f, 0.0f, 0.0f},
			{0.6875f, 0.0f, 0.0f},
			{0.65625f, 0.0f, 0.0f},
			{0.625f, 0.0f, 0.0f},
			{0.59375f, 0.0f, 0.0f},
			{0.5625f, 0.0f, 0.0f},
			{0.53125f, 0.0f, 0.0f},
			{0.5f, 0.0f, 0.0f}};

	void colorDepthImage(cv::Mat &imgDepth, cv::Mat &imgColoredDepth,
						 float minRange, float maxRange)
	{
		imgColoredDepth = cv::Mat::zeros(imgDepth.size(), CV_8UC3);

		for (int i = 0; i < imgColoredDepth.rows; ++i)
		{
			const float *depth = imgDepth.ptr<float>(i);
			unsigned char *pixel = imgColoredDepth.ptr<unsigned char>(i);
			for (int j = 0; j < imgColoredDepth.cols; ++j)
			{
				if (depth[j] != 0)
				{
					int idx = fminf(depth[j] - minRange, maxRange - minRange) / (maxRange - minRange) * 127.0f;
					idx = 127 - idx;

					pixel[0] = colormapJet[idx][2] * 255.0f;
					pixel[1] = colormapJet[idx][1] * 255.0f;
					pixel[2] = colormapJet[idx][0] * 255.0f;
				}

				pixel += 3;
			}
		}
	}

	bool colormap(const std::string &name, unsigned char idx,
				  float &r, float &g, float &b)
	{
		if (name.compare("jet") == 0)
		{
			float *color = colormapJet[idx];

			r = color[0];
			g = color[1];
			b = color[2];

			return true;
		}
		else if (name.compare("autumn") == 0)
		{
			float *color = colormapAutumn[idx];

			r = color[0];
			g = color[1];
			b = color[2];

			return true;
		}

		return false;
	}

	std::vector<cv::Point2i> bresLine(int x0, int y0, int x1, int y1)
	{
		// Bresenham's line algorithm
		// Find cells intersected by line between (x0,y0) and (x1,y1)

		std::vector<cv::Point2i> cells;

		int dx = std::abs(x1 - x0);
		int dy = std::abs(y1 - y0);

		int sx = (x0 < x1) ? 1 : -1;
		int sy = (y0 < y1) ? 1 : -1;

		int err = dx - dy;

		while (1)
		{
			cells.push_back(cv::Point2i(x0, y0));

			if (x0 == x1 && y0 == y1)
			{
				break;
			}

			int e2 = 2 * err;
			if (e2 > -dy)
			{
				err -= dy;
				x0 += sx;
			}
			if (e2 < dx)
			{
				err += dx;
				y0 += sy;
			}
		}

		return cells;
	}

	std::vector<cv::Point2i> bresCircle(int x0, int y0, int r)
	{
		// Bresenham's circle algorithm
		// Find cells intersected by circle with center (x0,y0) and radius r

		std::vector<std::vector<bool>> mask(2 * r + 1);

		for (int i = 0; i < 2 * r + 1; ++i)
		{
			mask[i].resize(2 * r + 1);
			for (int j = 0; j < 2 * r + 1; ++j)
			{
				mask[i][j] = false;
			}
		}

		int f = 1 - r;
		int ddF_x = 1;
		int ddF_y = -2 * r;
		int x = 0;
		int y = r;

		std::vector<cv::Point2i> line;

		line = bresLine(x0, y0 - r, x0, y0 + r);
		for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)
		{
			mask[it->x - x0 + r][it->y - y0 + r] = true;
		}

		line = bresLine(x0 - r, y0, x0 + r, y0);
		for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)
		{
			mask[it->x - x0 + r][it->y - y0 + r] = true;
		}

		while (x < y)
		{
			if (f >= 0)
			{
				y--;
				ddF_y += 2;
				f += ddF_y;
			}

			x++;
			ddF_x += 2;
			f += ddF_x;

			line = bresLine(x0 - x, y0 + y, x0 + x, y0 + y);
			for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)
			{
				mask[it->x - x0 + r][it->y - y0 + r] = true;
			}

			line = bresLine(x0 - x, y0 - y, x0 + x, y0 - y);
			for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)
			{
				mask[it->x - x0 + r][it->y - y0 + r] = true;
			}

			line = bresLine(x0 - y, y0 + x, x0 + y, y0 + x);
			for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)
			{
				mask[it->x - x0 + r][it->y - y0 + r] = true;
			}

			line = bresLine(x0 - y, y0 - x, x0 + y, y0 - x);
			for (std::vector<cv::Point2i>::iterator it = line.begin(); it != line.end(); ++it)
			{
				mask[it->x - x0 + r][it->y - y0 + r] = true;
			}
		}

		std::vector<cv::Point2i> cells;
		for (int i = 0; i < 2 * r + 1; ++i)
		{
			for (int j = 0; j < 2 * r + 1; ++j)
			{
				if (mask[i][j])
				{
					cells.push_back(cv::Point2i(i - r + x0, j - r + y0));
				}
			}
		}

		return cells;
	}

	void
	fitCircle(const std::vector<cv::Point2d> &points,
			  double &centerX, double &centerY, double &radius)
	{
		// D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data,
		// IEEE Transactions on Instrumentation and Measurement, 2000
		// We use the modified least squares method.
		double sum_x = 0.0;
		double sum_y = 0.0;
		double sum_xx = 0.0;
		double sum_xy = 0.0;
		double sum_yy = 0.0;
		double sum_xxx = 0.0;
		double sum_xxy = 0.0;
		double sum_xyy = 0.0;
		double sum_yyy = 0.0;

		int n = points.size();
		for (int i = 0; i < n; ++i)
		{
			double x = points.at(i).x;
			double y = points.at(i).y;

			sum_x += x;
			sum_y += y;
			sum_xx += x * x;
			sum_xy += x * y;
			sum_yy += y * y;
			sum_xxx += x * x * x;
			sum_xxy += x * x * y;
			sum_xyy += x * y * y;
			sum_yyy += y * y * y;
		}

		double A = n * sum_xx - square(sum_x);
		double B = n * sum_xy - sum_x * sum_y;
		double C = n * sum_yy - square(sum_y);
		double D = 0.5 * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx);
		double E = 0.5 * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy);

		centerX = (D * C - B * E) / (A * C - square(B));
		centerY = (A * E - B * D) / (A * C - square(B));

		double sum_r = 0.0;
		for (int i = 0; i < n; ++i)
		{
			double x = points.at(i).x;
			double y = points.at(i).y;

			sum_r += hypot(x - centerX, y - centerY);
		}

		radius = sum_r / n;
	}

	std::vector<cv::Point2d>
	intersectCircles(double x1, double y1, double r1,
					 double x2, double y2, double r2)
	{
		std::vector<cv::Point2d> ipts;

		double d = hypot(x1 - x2, y1 - y2);
		if (d > r1 + r2)
		{
			// circles are separate
			return ipts;
		}
		if (d < fabs(r1 - r2))
		{
			// one circle is contained within the other
			return ipts;
		}

		double a = (square(r1) - square(r2) + square(d)) / (2.0 * d);
		double h = sqrt(square(r1) - square(a));

		double x3 = x1 + a * (x2 - x1) / d;
		double y3 = y1 + a * (y2 - y1) / d;

		if (h < 1e-10)
		{
			// two circles touch at one point
			ipts.push_back(cv::Point2d(x3, y3));
			return ipts;
		}

		ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d,
								   y3 - h * (x2 - x1) / d));
		ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d,
								   y3 + h * (x2 - x1) / d));
		return ipts;
	}

	char
	UTMLetterDesignator(double latitude)
	{
		// This routine determines the correct UTM letter designator for the given latitude
		// returns 'Z' if latitude is outside the UTM limits of 84N to 80S
		// Written by Chuck Gantz- chuck.gantz@globalstar.com
		char letterDesignator;

		if ((84.0 >= latitude) && (latitude >= 72.0))
			letterDesignator = 'X';
		else if ((72.0 > latitude) && (latitude >= 64.0))
			letterDesignator = 'W';
		else if ((64.0 > latitude) && (latitude >= 56.0))
			letterDesignator = 'V';
		else if ((56.0 > latitude) && (latitude >= 48.0))
			letterDesignator = 'U';
		else if ((48.0 > latitude) && (latitude >= 40.0))
			letterDesignator = 'T';
		else if ((40.0 > latitude) && (latitude >= 32.0))
			letterDesignator = 'S';
		else if ((32.0 > latitude) && (latitude >= 24.0))
			letterDesignator = 'R';
		else if ((24.0 > latitude) && (latitude >= 16.0))
			letterDesignator = 'Q';
		else if ((16.0 > latitude) && (latitude >= 8.0))
			letterDesignator = 'P';
		else if ((8.0 > latitude) && (latitude >= 0.0))
			letterDesignator = 'N';
		else if ((0.0 > latitude) && (latitude >= -8.0))
			letterDesignator = 'M';
		else if ((-8.0 > latitude) && (latitude >= -16.0))
			letterDesignator = 'L';
		else if ((-16.0 > latitude) && (latitude >= -24.0))
			letterDesignator = 'K';
		else if ((-24.0 > latitude) && (latitude >= -32.0))
			letterDesignator = 'J';
		else if ((-32.0 > latitude) && (latitude >= -40.0))
			letterDesignator = 'H';
		else if ((-40.0 > latitude) && (latitude >= -48.0))
			letterDesignator = 'G';
		else if ((-48.0 > latitude) && (latitude >= -56.0))
			letterDesignator = 'F';
		else if ((-56.0 > latitude) && (latitude >= -64.0))
			letterDesignator = 'E';
		else if ((-64.0 > latitude) && (latitude >= -72.0))
			letterDesignator = 'D';
		else if ((-72.0 > latitude) && (latitude >= -80.0))
			letterDesignator = 'C';
		else
			letterDesignator = 'Z'; // This is here as an error flag to show that the Latitude is outside the UTM limits

		return letterDesignator;
	}

	void
	LLtoUTM(double latitude, double longitude,
			double &utmNorthing, double &utmEasting, std::string &utmZone)
	{
		// converts lat/long to UTM coords.  Equations from USGS Bulletin 1532
		// East Longitudes are positive, West longitudes are negative.
		// North latitudes are positive, South latitudes are negative
		// Lat and Long are in decimal degrees
		// Written by Chuck Gantz- chuck.gantz@globalstar.com

		double k0 = 0.9996;

		double LongOrigin;
		double eccPrimeSquared;
		double N, T, C, A, M;

		double LatRad = latitude * M_PI / 180.0;
		double LongRad = longitude * M_PI / 180.0;
		double LongOriginRad;

		int ZoneNumber = static_cast<int>((longitude + 180.0) / 6.0) + 1;

		if (latitude >= 56.0 && latitude < 64.0 &&
			longitude >= 3.0 && longitude < 12.0)
		{
			ZoneNumber = 32;
		}

		// Special zones for Svalbard
		if (latitude >= 72.0 && latitude < 84.0)
		{
			if (longitude >= 0.0 && longitude < 9.0)
				ZoneNumber = 31;
			else if (longitude >= 9.0 && longitude < 21.0)
				ZoneNumber = 33;
			else if (longitude >= 21.0 && longitude < 33.0)
				ZoneNumber = 35;
			else if (longitude >= 33.0 && longitude < 42.0)
				ZoneNumber = 37;
		}
		LongOrigin = static_cast<double>((ZoneNumber - 1) * 6 - 180 + 3); //+3 puts origin in middle of zone
		LongOriginRad = LongOrigin * M_PI / 180.0;

		// compute the UTM Zone from the latitude and longitude
		std::ostringstream oss;
		oss << ZoneNumber << UTMLetterDesignator(latitude);
		utmZone = oss.str();

		eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);

		N = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(LatRad) * sin(LatRad));
		T = tan(LatRad) * tan(LatRad);
		C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
		A = cos(LatRad) * (LongRad - LongOriginRad);

		M = WGS84_A * ((1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0) * LatRad - (3.0 * WGS84_ECCSQ / 8.0 + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0 + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) * sin(2.0 * LatRad) + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0 + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0) * sin(4.0 * LatRad) - (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0) * sin(6.0 * LatRad));

		utmEasting = k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0 + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * eccPrimeSquared) * A * A * A * A * A / 120.0) + 500000.0;

		utmNorthing = k0 * (M + N * tan(LatRad) *
									(A * A / 2.0 +
									 (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0 + (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * eccPrimeSquared) * A * A * A * A * A * A / 720.0));
		if (latitude < 0.0)
		{
			utmNorthing += 10000000.0; // 10000000 meter offset for southern hemisphere
		}
	}

	void
	UTMtoLL(double utmNorthing, double utmEasting, const std::string &utmZone,
			double &latitude, double &longitude)
	{
		// converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
		// East Longitudes are positive, West longitudes are negative.
		// North latitudes are positive, South latitudes are negative
		// Lat and Long are in decimal degrees.
		// Written by Chuck Gantz- chuck.gantz@globalstar.com

		double k0 = 0.9996;
		double eccPrimeSquared;
		double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ));
		double N1, T1, C1, R1, D, M;
		double LongOrigin;
		double mu, phi1, phi1Rad;
		double x, y;
		int ZoneNumber;
		char ZoneLetter;
		bool NorthernHemisphere;

		x = utmEasting - 500000.0; // remove 500,000 meter offset for longitude
		y = utmNorthing;

		std::istringstream iss(utmZone);
		iss >> ZoneNumber >> ZoneLetter;
		if ((static_cast<int>(ZoneLetter) - static_cast<int>('N')) >= 0)
		{
			NorthernHemisphere = true; // point is in northern hemisphere
		}
		else
		{
			NorthernHemisphere = false; // point is in southern hemisphere
			y -= 10000000.0;			// remove 10,000,000 meter offset used for southern hemisphere
		}

		LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + 3.0; //+3 puts origin in middle of zone

		eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);

		M = y / k0;
		mu = M / (WGS84_A * (1.0 - WGS84_ECCSQ / 4.0 - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0 - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0));

		phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu) + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0) * sin(4.0 * mu) + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu);
		phi1 = phi1Rad / M_PI * 180.0;

		N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad));
		T1 = tan(phi1Rad) * tan(phi1Rad);
		C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
		R1 = WGS84_A * (1.0 - WGS84_ECCSQ) /
			 pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5);
		D = x / (N1 * k0);

		latitude = phi1Rad - (N1 * tan(phi1Rad) / R1) * (D * D / 2.0 - (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1 - 9.0 * eccPrimeSquared) * D * D * D * D / 24.0 + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1 - 252.0 * eccPrimeSquared - 3.0 * C1 * C1) * D * D * D * D * D * D / 720.0);
		latitude *= 180.0 / M_PI;

		longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0 + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1 + 8.0 * eccPrimeSquared + 24.0 * T1 * T1) * D * D * D * D * D / 120.0) / cos(phi1Rad);
		longitude = LongOrigin + longitude / M_PI * 180.0;
	}

	long int
	timestampDiff(uint64_t t1, uint64_t t2)
	{
		if (t2 > t1)
		{
			uint64_t d = t2 - t1;

			if (d > std::numeric_limits<long int>::max())
			{
				return std::numeric_limits<long int>::max();
			}
			else
			{
				return d;
			}
		}
		else
		{
			uint64_t d = t1 - t2;

			if (d > std::numeric_limits<long int>::max())
			{
				return std::numeric_limits<long int>::min();
			}
			else
			{
				return -static_cast<long int>(d);
			}
		}
	}

}
